#!/usr/bin/env python

PACKAGE = "stepper_arm"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("Pick_start", bool_t, 0, "Whether use dynamic HSV threshold", False)

gen.add("car_search_distance_max", double_t, 0, "", 3.0, -20.0, 20.0)

gen.add("grab_start_jointA", double_t, 0, "", 2.0, -2.2, 2.2)
gen.add("grab_start_position_x", double_t, 0, "", 0.2, 0.0, 0.27)
gen.add("grab_start_position_y", double_t, 0, "", -0.008, -0.18, 0.24)

gen.add("grab_end_jointA", double_t, 0, "", -2.0, -2.2, 2.2)
gen.add("grab_end_position_x", double_t, 0, "", 0.25, 0.0, 0.27)
gen.add("grab_end_position_y", double_t, 0, "", -0.145, -0.18, 0.24)


gen.add("hand_open_size", double_t, 0, "the arm_hand open scale", -0.7, -1.0, 1.0)
gen.add("hand_close_size", double_t, 0, "the arm_hand close scale", 0.7, -1.0, 1.0)

color_enum = gen.enum([ gen.const("Dynamic", int_t, 0, "Use dynamic threshold"),
	                    gen.const("Green",   int_t, 1, "Preset HSV threshold of Green"),                    
                        gen.const("Blue",    int_t, 2, "Preset HSV threshold of Blue"),
                        gen.const("Yellow",  int_t, 3, "Preset HSV threshold of Yellow"),
                        gen.const("Red",     int_t, 4, "Preset HSV threshold of Red")], "Preset HSV threshold list")
gen.add("color", int_t, 0, "chose preset HSV threshold", 0, 0, 4, edit_method=color_enum)

gen.add("HSV_H_MIN", int_t, 0, "HSV_H_MIN", 0, 0, 255)
gen.add("HSV_S_MIN", int_t, 0, "HSV_S_MIN", 0, 0, 255)
gen.add("HSV_V_MIN", int_t, 0, "HSV_V_MIN", 0, 0, 255)
gen.add("HSV_H_MAX", int_t, 0, "HSV_H_MAX", 0, 0, 255)
gen.add("HSV_S_MAX", int_t, 0, "HSV_S_MAX", 0, 0, 255)
gen.add("HSV_V_MAX", int_t, 0, "HSV_V_MAX", 0, 0, 255)

gen.add("dilate_erode_size", int_t, 0, "dilate_erode_size", 7, 0, 25)

gen.add("car_search_foward", double_t, 0, "the speed of search color block", 0.25, -2.0, 2.0)

gen.add("target_areaSize", double_t, 0, "present distance", 19000.0, 0.0, 50000.0)

gen.add("target_y_bias", double_t, 0, "target_y_bias", 60, -120, 120)

gen.add("Move_step_max", double_t, 0, "Limit PARAM", 0.4,    -2.0, 2.0)
gen.add("Turn_step_max", double_t, 0, "Limit PARAM", 0.2,    -2.0, 2.0)
gen.add("Move_step_min", double_t, 0, "Limit PARAM", 0.0005, -2.0, 2.0)
gen.add("Turn_step_min", double_t, 0, "Limit PARAM", 0.0005, -2.0, 2.0)

gen.add("turn_KP", double_t, 0, "X PID PARAM", 0.005, -2.0, 2.0)

gen.add("turn_KD", double_t, 0, "X PID PARAM", 0.0, -2.0, 2.0)

gen.add("down_KP", double_t, 0, "Distance PID PARAM", 0.1, -2.0, 2.0)

gen.add("down_KD", double_t, 0, "Distance PID PARAM", 0.00, -2.0, 2.0)

gen.add("foward_KP", double_t, 0, "Y PID PARAM", 0.15, -1.0, 1.0)

gen.add("foward_KD", double_t, 0, "Y PID PARAM", 0.0, -1.0, 1.0)

gen.add("Car_foward_KP", double_t, 0, "Car_foward PID PARAM", 0.1, -1.0, 1.0)

# Second arg is node name it will run in (doc purposes only), third is generated filename prefix
exit(gen.generate(PACKAGE, "stepper_arm", "arm_color_block_params"))

